#include "path_follow_controller.h"

void PathFollowController::getOut(float &lianer_vel, float &steer_angle) {
    lianer_vel  = linear_vel_;
    steer_angle = steer_angle_;
};

void PathFollowController::startFollow(std::vector<Point> path) {
    if (path.empty()) {
        std::cerr << "empty path" << std::endl;
        follow_state_ = FollowState::kFailed;
    }

    path_ = path;

    // 开启跟随线程
    std::thread start_follow_thread(&PathFollowController::FollowThread, this);
    start_follow_thread.detach();
};

FollowState PathFollowController::getState() {
    return follow_state_;
};